#include <ros/ros.h>
#include <signal.h>
#include <geometry_msgs/Twist.h>
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <string.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <visualization_msgs/Marker.h>
#include <cmath>
#include <vector>

using namespace std;

ros::Publisher cmd_vel_pub;
ros::Publisher marker_pub;
ros::Subscriber odom_sub;

geometry_msgs::Pose cur_pose;
bool pose_initialized = false;

void shutdown(int sig);
void odomCallback(const nav_msgs::OdometryConstPtr &msg);
void init_markers(visualization_msgs::Marker *marker);
void getPoseListSquare(float unit_len, vector<geometry_msgs::Pose> &pose_list, visualization_msgs::Marker &marker);
void getPoseListLine(float unit_len, vector<geometry_msgs::Pose> &pose_list, visualization_msgs::Marker &marker);
void getPoseListCurve(float unit_len, vector<geometry_msgs::Pose> &pose_list, visualization_msgs::Marker &marker);
int main(int argc, char** argv)
{
  ros::init(argc, argv, "nav_move_base");
  std::string topic = "/cmd_vel";
  ros::NodeHandle node("~");

  float unit_len = 1.4*5;
  int traj_type = 1; // 0 line;1 square; 2 compound
  node.param<float>("unit_len",unit_len,7.0);
  node.param<int>("traj_type",traj_type,1);

  cout<<unit_len<<endl;
  odom_sub = node.subscribe<nav_msgs::Odometry>("/kloam/mapping/odometry_fusion",100,odomCallback);
  //Subscribe to the move_base action server
  actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> ac("move_base",true);
  //Define a marker publisher.
  marker_pub = node.advertise<visualization_msgs::Marker>("waypoint_markers", 10);
  //Publisher to manually control the robot (e.g. to stop it, queue_size=5)
  cmd_vel_pub = node.advertise<geometry_msgs::Twist>(topic, 5);

  //for init_markers function
  visualization_msgs::Marker marker;
  vector<geometry_msgs::Pose> pose_list;

  signal(SIGINT, shutdown);
  ROS_INFO("Waiting for move_base action server...");
  //Wait 60 seconds for the action server to become available
  // if(!ac.waitForServer(ros::Duration(60)))
  // {
  //   ROS_INFO("Can't connected to move base server");
  //   return 1;
  // }
  // ROS_INFO("Connected to move base server");
  ROS_INFO("Starting navigation test");

  //Cycle through the four waypoints
  int count = 0;

  while(true)
  {
    ros::spinOnce();
    if(pose_initialized == false)
    {
      // cout<<"still not initialized"<<endl;
      continue;
    }
    else
    {
      // cout<<traj_type<<endl;
      switch (traj_type)
      {
        case 0: 
          getPoseListLine(unit_len,pose_list,marker);
          break;
        case 1: 
          getPoseListSquare(unit_len,pose_list,marker);
          break;
        case 2: 
          getPoseListCurve(unit_len,pose_list,marker);
          break;
        default:
          cout<<"not recognized type"<<endl;
      }
      break;
    }
  }

  cout<<"current pose xy "<<cur_pose.position.x<<" "<<cur_pose.position.y<<endl;
  cout<<"pose size "<<pose_list.size()<<endl;
  while(ros::ok())
  {
     //Update the marker display
     marker_pub.publish(marker);
     //Intialize the waypoint goal
     move_base_msgs::MoveBaseGoal goal;
     //Use the map frame to define goal poses
     goal.target_pose.header.frame_id = "map";
     //Set the time stamp to "now"
     goal.target_pose.header.stamp = ros::Time::now();
     //Set the goal pose to the i-th waypoint
     goal.target_pose.pose = pose_list[count];

     cout<<"current goal xy "<<goal.target_pose.pose.position.x
      <<" "<<goal.target_pose.pose.position.y<<endl;
     //Start the robot moving toward the goal
     //Send the goal pose to the MoveBaseAction server
     ac.sendGoal(goal);
    //Allow 3 minute to get there
    bool finished_within_time = ac.waitForResult(ros::Duration(30));
    //If we dont get there in time, abort the goal
    if(!finished_within_time)
    {
        ac.cancelGoal();
        ROS_INFO("Timed out achieving goal");
    }
    else
    {
        //We made it!
        if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
        {
            ROS_INFO("Goal succeeded!");
        }
        else
        {
            ROS_INFO("The base failed for some reason");
        }
    }
    count += 1;
    if(count >= pose_list.size()) break;
  }
  ROS_INFO("move_base_square.cpp end...");
  return 0;
}


void shutdown(int sig)
{
  cmd_vel_pub.publish(geometry_msgs::Twist());
  ros::Duration(1).sleep(); // sleep for  a second
  ROS_INFO("nav_square.cpp ended!");
  ros::shutdown();
}

void odomCallback(const nav_msgs::OdometryConstPtr &msg)
{
  if(pose_initialized == false){
    cur_pose.position = msg->pose.pose.position;
    cur_pose.orientation = msg->pose.pose.orientation;
    pose_initialized = true;
  }
}

void init_markers(visualization_msgs::Marker *marker)
{
  marker->ns       = "waypoints";
  marker->id       = 0;
  marker->type     = visualization_msgs::Marker::CUBE_LIST;
  marker->action   = visualization_msgs::Marker::ADD;
  marker->lifetime = ros::Duration();//0 is forever
  marker->scale.x  = 0.2;
  marker->scale.y  = 0.2;
  marker->color.r  = 1.0;
  marker->color.g  = 0.7;
  marker->color.b  = 1.0;
  marker->color.a  = 1.0;

  marker->header.frame_id = "odom";
  marker->header.stamp = ros::Time::now();

}

void getPoseListSquare(float unit_len, vector<geometry_msgs::Pose> &pose_list, visualization_msgs::Marker &marker)
{
  //a pose consisting of a position and orientation in the map frame.
  geometry_msgs::Pose geo_pose;
  float roll,pitch,cur_yaw;
  cur_yaw = tf::getYaw(cur_pose.orientation);
  cout<<"cur yaw: "<<cur_yaw<<endl;

  geo_pose = cur_pose;

  geo_pose.position.x += unit_len*cos(cur_yaw);
  geo_pose.position.y += unit_len*sin(cur_yaw);
  geo_pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, cur_yaw);;
  pose_list.push_back(geo_pose);

  geo_pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, cur_yaw+M_PI/2);;
  pose_list.push_back(geo_pose);

  geo_pose.position.x -=  unit_len*sin(cur_yaw);
  geo_pose.position.y +=  unit_len*cos(cur_yaw);
  geo_pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, cur_yaw+M_PI/2);
  pose_list.push_back(geo_pose);

  geo_pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, cur_yaw+2*M_PI/2);
  pose_list.push_back(geo_pose);

  geo_pose.position.x -=  unit_len*cos(cur_yaw);
  geo_pose.position.y -=  unit_len*sin(cur_yaw);
  geo_pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, cur_yaw+2*M_PI/2);
  pose_list.push_back(geo_pose);

  //6  
  geo_pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, cur_yaw+3*M_PI/2);
  pose_list.push_back(geo_pose);

  geo_pose.position.x +=  unit_len*cos(cur_yaw);
  geo_pose.position.y -=  unit_len*cos(cur_yaw);
  geo_pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, cur_yaw+3*M_PI/2);
  pose_list.push_back(geo_pose);

  geo_pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, cur_yaw);
  pose_list.push_back(geo_pose);

  init_markers(&marker);

  //Set a visualization marker at each waypoint
  for(int i = 0; i < pose_list.size(); i++)
  {
    marker.points.push_back(pose_list[i].position);
  }

}

void getPoseListLine(float unit_len, vector<geometry_msgs::Pose> &pose_list, visualization_msgs::Marker &marker)
{
  //a pose consisting of a position and orientation in the map frame.
  geometry_msgs::Pose geo_pose;
  float roll,pitch,cur_yaw;
  cur_yaw = tf::getYaw(cur_pose.orientation);
  cout<<"cur yaw: "<<cur_yaw<<endl;

  geo_pose = cur_pose;

  geo_pose.position.x += unit_len*cos(cur_yaw);
  geo_pose.position.y += unit_len*sin(cur_yaw);
  geo_pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, cur_yaw);;
  pose_list.push_back(geo_pose);

  init_markers(&marker);

  //Set a visualization marker at each waypoint
  marker.points.push_back(pose_list[0].position);

}

void getPoseListCurve(float unit_len, vector<geometry_msgs::Pose> &pose_list, visualization_msgs::Marker &marker)
{
  //a pose consisting of a position and orientation in the map frame.
  geometry_msgs::Pose geo_pose;
  float roll,pitch,cur_yaw;
  cur_yaw = tf::getYaw(cur_pose.orientation);
  cout<<"cur yaw: "<<cur_yaw<<endl;
  geo_pose = cur_pose;

  geo_pose.position.x += unit_len*cos(cur_yaw);
  geo_pose.position.y += unit_len*sin(cur_yaw);
  geo_pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, cur_yaw);;
  pose_list.push_back(geo_pose);

  geo_pose.position.x +=  unit_len*cos(cur_yaw);
  geo_pose.position.x -=  unit_len*sin(cur_yaw);
  geo_pose.position.y +=  unit_len*sin(cur_yaw);
  geo_pose.position.y +=  unit_len*cos(cur_yaw);
  geo_pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, cur_yaw+M_PI/2);
  pose_list.push_back(geo_pose);

  init_markers(&marker);
  //Set a visualization marker at each waypoint
  for(int i = 0; i < pose_list.size(); i++)
  {
    marker.points.push_back(pose_list[i].position);
  }

}